#include "pid.h"


PID_Ctrl_t PID_User;


void PID_Init(PID_Ctrl_t* Ptr ,float setpoint,float p, float i, float d, float max_output, float min_output, float max_integral, float min_integral)
{
    Ptr->Kp = p; // 比例系数
    Ptr->Ki = i; // 积分系数
    Ptr->Kd = d; // 微分系数
    Ptr->Max_output = max_output; // 输出限幅
    Ptr->Min_output = min_output; // 输出限幅
    Ptr->Max_integral = max_integral; // 积分限幅
    Ptr->Min_integral = min_integral; // 积分限幅
    Ptr->last_input = 0; // 上次输入值
    Ptr->input = 0; // 输入值
    Ptr->setpoint = setpoint; // 设定值
}

void PID_Ctrl(PID_Ctrl_t* Ptr)
{
    Ptr->error_val =Ptr->setpoint -  Ptr->input ; // 计算误差值
    Ptr->error_integral += Ptr->error_val; // 计算误差积分值
    Ptr->error_derivative = Ptr->last_input - Ptr->input; // 计算误差微分值
    Ptr->last_input = Ptr->input; // 更新上次输入值
    
    //积分限幅
    if (Ptr->error_integral > Ptr->Max_integral)
    {
        Ptr->error_integral = Ptr->Max_integral;
    }
    else if (Ptr->error_integral < Ptr->Min_integral)
    {
        Ptr->error_integral = Ptr->Min_integral;
    }

    //控制器
    Ptr->output = Ptr->Kp * Ptr->error_val + Ptr->Ki * Ptr->error_integral + Ptr->Kd * Ptr->error_derivative; // PID控制器计算
    //输出限幅
    if (Ptr->output > Ptr->Max_output)
    {
        Ptr->output = Ptr->Max_output;
    }
    else if (Ptr->output < Ptr->Min_output)
    {
        Ptr->output = Ptr->Min_output;
    }    
}

